#include"myPredict.h"

void update_vector(vector<double>& the_vector,int length_limit,double new_value)
{
    if (the_vector.size()>=length_limit)
    {
        the_vector.erase(the_vector.begin());
    }
    the_vector.push_back(new_value);
}

void Abs_info1::clear_info()
{
    abs_last_pitch=0;
    abs_last_yaw=0;
    abs_pitch_vs.clear();
    abs_yaw_vs.clear();
    abs_pitch_as.clear();
    abs_yaw_as.clear();
}


my_predict::my_predict()
{
    init_flag=0;
    init_kalman();
}

void my_predict::init_kalman()
{
    kf_pos.SetPredictError(1e-2);
    kf_pos.SetComputeError(1);
    kf_v.SetPredictError(1);
    kf_v.SetComputeError(1);
}

void my_predict::reset_predictor()
{
    init_flag=0;
    find_cnt=0;
    abs_info.clear_info();
    kf_pos.ReInitAllState();
    kf_v.ReInitAllState();

}

void my_predict::init_coordinate()
{
    if(init_flag==0)
    {
        cout<<"init_coordinate "<<endl;
        abs_info.clear_info();
        init_flag=1;
    }
}

void my_predict::update_abs_info(double armor_pitch,double armor_yaw)
{
    find_cnt+=1;
    double abs_pitch,abs_yaw,abs_pitch_v,abs_yaw_v,abs_pitch_a,abs_yaw_a;
    double last_pitch_v,last_yaw_v;

    abs_pitch=armor_pitch;
    abs_yaw=armor_yaw;

    vector<double> now_py=kf_pos.GetnowPoint(abs_pitch,abs_yaw);
    abs_pitch=now_py[0];
    abs_yaw=now_py[1];

    vector<double> py_speed = kf_v.GetnowPoint((double)now_py[2], (double)now_py[3]);

    if(find_cnt<40)
    {
        abs_pitch_v = armor_pitch - abs_info.abs_last_pitch;
        abs_yaw_v = armor_yaw - abs_info.abs_last_yaw;
        abs_pitch_a = 0;
        abs_yaw_a = 0;
        abs_info.abs_last_pitch=armor_pitch;
        abs_info.abs_last_yaw=armor_yaw;
        update_vector(abs_info.abs_pitch_vs,SPEED_VECTOR_LIMIT,abs_pitch_v);
        update_vector(abs_info.abs_yaw_vs,SPEED_VECTOR_LIMIT,abs_yaw_v);
    }
    else
    {
        abs_pitch_v = py_speed[0];
        abs_yaw_v = py_speed[1];
        abs_pitch_a = py_speed[2];
        abs_yaw_a = py_speed[3];
        abs_info.abs_last_pitch=abs_pitch;
        abs_info.abs_last_yaw=abs_yaw;
        update_vector(abs_info.abs_pitch_vs,SPEED_VECTOR_LIMIT,abs_pitch_v);
        update_vector(abs_info.abs_yaw_vs,SPEED_VECTOR_LIMIT,abs_yaw_v);
    }


}


void my_predict::predict_next_pos(int drop_frame_cnt)
{

    pitch_out=abs_info.abs_last_pitch+abs_info.abs_pitch_vs.back()*drop_frame_cnt;
    yaw_out=abs_info.abs_last_yaw+abs_info.abs_yaw_vs.back();


}
